#include "tools.h"
namespace Registration {
__device__ float3 transform(float3 target_point, float *rotate_matrix,
                            float *translation_matrix) {
  float3 rotate_point;
  rotate_point.x = rotate_matrix[0] * target_point.x +
                   rotate_matrix[1] * target_point.y +
                   rotate_matrix[2] * target_point.z;
  rotate_point.y = rotate_matrix[3] * target_point.x +
                   rotate_matrix[4] * target_point.y +
                   rotate_matrix[5] * target_point.z;
  rotate_point.z = rotate_matrix[6] * target_point.x +
                   rotate_matrix[7] * target_point.y +
                   rotate_matrix[8] * target_point.z;
  float3 result_point = make_float3(rotate_point.x + translation_matrix[0],
                                    rotate_point.y + translation_matrix[1],
                                    rotate_point.z + translation_matrix[2]);
  return result_point;
}
__device__ float norm(float3 input_vector) {
  float length =
      sqrt(input_vector.x * input_vector.x + input_vector.y * input_vector.y +
           input_vector.z * input_vector.z);
  return length;
}
__device__ float3 rotate(float3 target_point, const float *rotate_matrix) {
  float3 rotate_point;
  rotate_point.x = rotate_matrix[0] * target_point.x +
                   rotate_matrix[1] * target_point.y +
                   rotate_matrix[2] * target_point.z;
  rotate_point.y = rotate_matrix[3] * target_point.x +
                   rotate_matrix[4] * target_point.y +
                   rotate_matrix[5] * target_point.z;
  rotate_point.z = rotate_matrix[6] * target_point.x +
                   rotate_matrix[7] * target_point.y +
                   rotate_matrix[8] * target_point.z;
  return rotate_point;
}
} // namespace Registration